package sdp;

import java.awt.image.BufferedImage;
import java.awt.image.ColorModel;
import java.awt.image.WritableRaster;
import java.util.List;

import com.googlecode.javacv.cpp.opencv_core.IplImage;

import au.edu.jcu.v4l4j.CaptureCallback;
import au.edu.jcu.v4l4j.Control;
import au.edu.jcu.v4l4j.FrameGrabber;
import au.edu.jcu.v4l4j.V4L4JConstants;
import au.edu.jcu.v4l4j.VideoDevice;
import au.edu.jcu.v4l4j.VideoFrame;
import au.edu.jcu.v4l4j.exceptions.V4L4JException;

import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.nio.ByteBuffer;

import lejos.pc.comm.NXTComm;
import lejos.pc.comm.NXTCommException;
import lejos.pc.comm.NXTCommFactory;
import lejos.pc.comm.NXTInfo;

public class Camera {
	public static final String videoDeviceName = "/dev/video0";

	public static class ConnectionFailed extends Exception {
		public ConnectionFailed() {
			
		}
	}
	
	public static class CommunicationError extends Exception {
		
	}
	
	public void setSaturation( int newSaturation ) {
		setCameraProperty( "Saturation", newSaturation );
	}
	
	public void setContrast( int newContrast ) {
		setCameraProperty( "Contrast", newContrast );
	}

	public void setBrightness( int newBrightness ) {
		setCameraProperty( "Brightness", newBrightness );
	}
	
	public void setChromaAGC( int newChromaAGC ) {
		setCameraProperty( "Chroma AGC", newChromaAGC );
	}
	
	public void setGain( int newGain ) {
		setCameraProperty( "Chroma Gain", newGain );
	}
	
	public void setHue( int newHue ) {
		setCameraProperty( "Hue", newHue );
	}
	
	public int getSaturation() throws CommunicationError {
		return getCameraProperty( "Saturation" );
	}
	
	public int getContrast() throws CommunicationError {
		return getCameraProperty( "Contrast" );
	}
	
	public int getBrightness() throws CommunicationError {
		return getCameraProperty( "Brightness" );
	}
	
	public int getHue() throws CommunicationError {
		return getCameraProperty( "Hue" );
	}
	
	public int getChromaAGC() throws CommunicationError  {
		return 0;//getCameraProperty( "Chroma AGC" );
	}
	
	public int getGain() throws CommunicationError  {
		return 0; //getCameraProperty( "Chroma Gain" );
	}
	
	public boolean setCameraProperty( String propertyName, int value ) {
		int status = 2;
		List<Control> controls = null;
		try {
			controls = videoDevice.getControlList().getList();
			for(Control c: controls) {
				if( c.getName().equals(propertyName) ) {
					c.setValue( value );
					status = 0;
					break;
				}
			}
		 } catch( V4L4JException e ) {
			 e.printStackTrace();
			 System.err.println("Camera: Could not set camera property '" + propertyName + "' to " + value);
			 status = 1;
		 } finally {
			 if( controls != null )
				 videoDevice.releaseControlList();
		 }
		 
		 if( status == 2 )
			 System.err.println("Camera: Could not find camera property '" + propertyName + "'");
		 
		 return status == 0;
	}
	
	public int getCameraProperty( String propertyName ) throws CommunicationError {
		List<Control> controls = null;
		try {
			controls = videoDevice.getControlList().getList();
			for(Control c: controls) {
				if( c.getName().equals(propertyName) )
					return c.getValue();
			}
		 } catch( V4L4JException e ) {
			 e.printStackTrace();
			 System.err.println("Camera: Could not set camera property '" + propertyName + "' to " );
			 throw new CommunicationError();
		 } finally {
			 if( controls != null )
				 videoDevice.releaseControlList();
		 }
		 
		 System.err.println("Camera: Could not find camera property '" + propertyName + "'");
		 throw new CommunicationError();
	}
	
	public double[] getEncoderValues() {
		return null;	
	}
	
	public boolean isBumperPressed() {
		return false;
	}
	
	public BufferedImage getCameraImage() throws V4L4JException {
		if( frame != null )
			frame.recycle();
		frame = frameGrabber.getVideoFrame();
		BufferedImage raw = frame.getBufferedImage();
		return raw;
	}
	
	public int getMotorCount() {
		return 0;
	}
	
	public boolean isConnected() {
		return cameraConnected;
	}
	
	public void connect() throws ConnectionFailed {
		connectCamera();
	}
	
	public void disconnect() {
		disconnectCamera();
		
	}
	
	private void connectCamera() throws ConnectionFailed {
		try {
			System.out.println("Camera: Opening video device '" + videoDeviceName + "'...");
			videoDevice  = new VideoDevice( videoDeviceName );
			
			System.out.println("Camera: Device opened. Adjustable properties:");
			List<Control> controls = null;
			try {
				controls = videoDevice.getControlList().getList();
				for(Control c: controls)
					System.out.println("Camera: " + c.getName() + " = " + c.getValue() + " in " + c.getMinValue() + "-" + c.getMaxValue());
			 } catch( V4L4JException e ) {
				 e.printStackTrace();
				 System.err.println("Camera: Could not get properties.");
				 throw new ConnectionFailed();
			 } finally {
				 if( controls != null )
					 videoDevice.releaseControlList();
			 }
			
			System.out.println("Camera: Creating frame grabber...");
			frameGrabber = videoDevice.getBGRFrameGrabber(720, 576, 0, V4L4JConstants.STANDARD_PAL);
			System.out.println("Camera: Starting capture");
			frameGrabber.startCapture();
		} catch( V4L4JException e ) {
			e.printStackTrace();
			System.err.println("Camera: Failed. Rolling back...");
			if( frameGrabber != null ) {
				videoDevice.releaseFrameGrabber();
				frameGrabber = null;
			}
			
			if( videoDevice != null ) {
				videoDevice.release();
				videoDevice = null;
			}
			
			throw new ConnectionFailed();
		}
		System.out.println("Camera: Camera connected.");
		cameraConnected = true;
	}
	
	private void disconnectCamera() {
		System.out.println("Camera: Disconnecting camera.");
		if( frameGrabber != null ) {
			frameGrabber.stopCapture();
			videoDevice.releaseFrameGrabber();
			frameGrabber = null;
		}
		
		if( videoDevice != null ) {
			videoDevice.release();
			videoDevice = null;
		}
		System.out.println("Camera: Camera disconnected.");
		cameraConnected = false;
	}
	
	
	
	private boolean       cameraConnected = false;
	private VideoDevice   videoDevice;
	private FrameGrabber  frameGrabber;
	private VideoFrame    frame;
}
